/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RW_GEOMETRY_OwnQHullND_HPP_
#define RW_GEOMETRY_OwnQHullND_HPP_

#include <stack>
#include <set>
#include <vector>
#include <float.h>
#include <boost/numeric/ublas/vector.hpp>
#include <rw/math/VectorND.hpp>
#include <rw/common/macros.hpp>
#include <rw/geometry/ConvexHullND.hpp>

namespace rw {
namespace geometry {
/** @addtogroup geometry
 *  @{
 *  @file OwnQHullND.hpp
 */

namespace OwnQHull {

/**
 * @brief calclates the convex hull of a set of vertices \b coords each with dimension  \b dim
 *
 * @param dim [in] nr of dimensions in each vertice
 * @param coords [in] array of vertices
 * @param nrCoords [in] the number of vertices
 * @return
 */
void build(size_t dim,
		double *coords,
		size_t nrCoords,
		std::vector<int>& vertIdxs,
		std::vector<int>& faceIdxs,
		std::vector<double>& faceNormals,
		std::vector<double>& faceOffsets);

void build_subspace(size_t dim,
		double *coords,
		size_t nrCoords,
		std::vector<rw::math::VectorND<3> >& returnVertices,
		std::vector<rw::math::VectorND<3> >& returnFaces,
		//std::vector<int>& vertIdxs,
		//std::vector<int>& faceIdxs,
		std::vector<double>& faceNormals,
		std::vector<double>& faceOffsets,
		const rw::math::VectorND<6> origo);
}

/**
 * @brief calculates the convex hull of a set of 3d points.
 *
 * The GirftWrap convex hull algorithm is used, hence the
 * class name.
 *
 * @note It is important that there are not multiple vertices at the same coordinates.
 * Filter these away before using this convex hull calculation.
 */
template <std::size_t N>
class OwnQHullND: public ConvexHullND<N> {
public:
	//static const std::size_t dimension = N;
	//typedef boost::numeric::ublas::bounded_vector<double, N> VectorND;
	//typedef int[N] FaceIdxND;

	/**
	 * @brief constructor
	 */
	OwnQHullND(){};

	/**
	 * @brief destructor
	 */
	virtual ~OwnQHullND(){};

	//! @copydoc ConvexHull3D::rebuild
	void rebuild(const std::vector<rw::math::VectorND<N> >& vertices){
		using namespace rw::math;
		// convert the vertice array to an array of double
		double *vertArray = new double[vertices.size()*N];
		// copy all data into the vertArray
		for(size_t i=0;i<vertices.size();i++){
			const VectorND<N> &vnd = vertices[i];
			for(size_t j=0;j<N;j++)
				vertArray[i*N+j] = vnd[j];
		}
		// build the hull
		OwnQHull::build(N, vertArray, vertices.size(), _vertiIdxs, _faceIdxs, _faceNormalsTmp, _faceOffsets);
		delete[] vertArray;

		_hullVertices.resize(_vertiIdxs.size());
		for(size_t i=0;i<_vertiIdxs.size(); i++){
			_hullVertices[i] = vertices[_vertiIdxs[i]];
		}
		_faceOffsets.resize(_faceIdxs.size()/N);
		_faceNormals.resize(_faceIdxs.size()/N);
		for(size_t i=0;i<_faceIdxs.size()/N; i++){
			for(size_t j=0; j<N; j++)
				_faceNormals[i][j] = _faceNormalsTmp[i*N+j];
		}
	}

	void subspace(const std::vector<rw::math::VectorND<N> >& vertices, const rw::math::VectorND<N> origo){
		using namespace rw::math;
		// convert the vertice array to an array of double
		double *vertArray = new double[vertices.size()*N];
		// copy all data into the vertArray
		for(size_t i=0;i<vertices.size();i++){
			const VectorND<N> &vnd = vertices[i];
			for(size_t j=0;j<N;j++)
				vertArray[i*N+j] = vnd[j];
		}
		std::vector<rw::math::VectorND<3> > returnVertices;
		// build the hull
		OwnQHull::build_subspace(N, vertArray, vertices.size(), returnVertices, _returnFaces, _faceNormalsTmp, _faceOffsets, origo);
		delete[] vertArray;

		/*_hullVertices.resize(_vertiIdxs.size());
		for(size_t i=0;i<_vertiIdxs.size(); i++){
			_hullVertices[i] = vertices[_vertiIdxs[i]];
		}*/
		_hullVertices.resize(returnVertices.size());
		for (size_t i=0; i<returnVertices.size();i++) {
			_hullVertices[i][0] = returnVertices[i][0];
			_hullVertices[i][1] = returnVertices[i][1];
			_hullVertices[i][2] = returnVertices[i][2];
			_hullVertices[i][3] = 0;
			_hullVertices[i][4] = 0;
			_hullVertices[i][5] = 0;
		}
		size_t dim = 3;
		_faceOffsets.resize(_returnFaces.size()/dim);
		_faceNormals.resize(_returnFaces.size()/dim);
		for(size_t i=0;i<_returnFaces.size()/dim; i++){
			for(size_t j=0; j<dim; j++)
				_faceNormals[i][j] = _faceNormalsTmp[i*dim+j];
		}
	}

	//! @copydoc ConvexHull3D::isInside
	bool isInside(const rw::math::VectorND<N>& vertex) { return 0; }

	//! @copydoc ConvexHull3D::isInside
	bool isInside(const rw::math::VectorND<N>& vertex, const std::vector<rw::math::VectorND<N> >& vertices){
		using namespace rw::math;
		//const static double EPSILON = 0.0000001;
		if( _faceIdxs.size()==0 ){
			//std::cout << "No Tris" << std::endl;
			return 0;
		}

		double minDist = DBL_MAX;
		for(size_t i=0; i<_faceIdxs.size()/N; i++){
			RW_ASSERT(_faceIdxs.size()> i*N);
			size_t faceVerticeIdx = _faceIdxs[i*N];
			RW_ASSERT(faceVerticeIdx<vertices.size());
			RW_ASSERT(i<_faceNormals.size());
			double dist =  _faceOffsets[i] + dot(vertex, _faceNormals[i]);
			// dist will be negative if point is inside, and positive if point is outside
			minDist = std::min( -dist, minDist );
			if(minDist<0)
				return false;
		}

		return minDist>=0;
	}

	//! @copydoc ConvexHull3D::getMinDistOutside
	double getMinDistOutside(const rw::math::VectorND<N>& vertex){ return 0; }

	//! @copydoc ConvexHull3D::getMinDistInside
	double getMinDistInside(const rw::math::VectorND<N>& vertex){
		using namespace rw::math;
		if( _faceIdxs.size()==0 ){
			return 0;
		}
		double minDist = DBL_MAX;
		for(size_t i=0; i<_faceIdxs.size()/N; i++){
			RW_ASSERT(_faceIdxs.size()> i*N);
			RW_ASSERT(i<_faceNormals.size());
			double dist =  _faceOffsets[i] + dot(vertex, _faceNormals[i]);
			// dist will be negative if point is inside, and positive if point is outside
			minDist = std::min( -dist, minDist );
		}
		return minDist;
	}

	rw::math::VectorND<N> getMinWrench(const rw::math::VectorND<N>& vertex){
		rw::math::VectorND<N> minWrench = rw::math::VectorND<N>::zero();
		using namespace rw::math;
		if( _faceIdxs.size()==0 ){
			return minWrench;
		}
		double minDist = DBL_MAX;
		for(size_t i=0; i<_faceIdxs.size()/N; i++){
			RW_ASSERT(_faceIdxs.size()> i*N);
			RW_ASSERT(i<_faceNormals.size());
			double dist =  _faceOffsets[i] + dot(vertex, _faceNormals[i]);
			// dist will be negative if point is inside, and positive if point is outside
			if (-dist < minDist) {
				minWrench[0] = 0;
				minWrench[1] = 0;
				minWrench[2] = 0;
				minWrench[3] = _faceNormals[i][3]*_faceOffsets[i];
				minWrench[4] = _faceNormals[i][4]*_faceOffsets[i];
				minWrench[5] = _faceNormals[i][5]*_faceOffsets[i];
			}
			minDist = std::min( -dist, minDist );
		}
		return minWrench;
	}

	double getMinDistInsideSubspace(const rw::math::VectorND<3>& vertex){
		using namespace rw::math;
		if( _returnFaces.size()==0 ){
			return 0;
		}
		double minDist = DBL_MAX;
		int dim = 3;
		for(size_t i=0; i<_returnFaces.size()/dim; i++){
			RW_ASSERT(_returnFaces.size()> i*dim);
			RW_ASSERT(i<_faceNormals.size());
			double dist =  _faceOffsets[i] + dot(vertex, _faceNormals[i]);
			// dist will be negative if point is inside, and positive if point is outside
			minDist = std::min( -dist, minDist );
		}
		return minDist;
	}

	//! if negative then point is outside hull
	double getMinDistInside(const rw::math::VectorND<N>& vertex, const std::vector<rw::math::VectorND<N> >& vertices){
		using namespace rw::math;
		if( _faceIdxs.size()==0 ){
			//std::cout << "No Tris" << std::endl;
			return 0;
		}
		double minDist = DBL_MAX;
		for(size_t i=0; i<_faceIdxs.size()/N; i++){
			RW_ASSERT(_faceIdxs.size()> i*N);
			size_t faceVerticeIdx = _faceIdxs[i*N];
			RW_ASSERT(faceVerticeIdx<vertices.size());
			RW_ASSERT(i<_faceNormals.size());
			double dist =  _faceOffsets[i] + dot(vertex, _faceNormals[i]);
			// dist will be negative if point is inside, and positive if point is outside
			minDist = std::min( -dist, minDist );
		}
		return minDist;
	}

	//! if negative then point is outside hull
	double getMinDistInsideSubspace(const rw::math::VectorND<3>& vertex, const std::vector<rw::math::VectorND<3> >& vertices){
		using namespace rw::math;
		if( _returnFaces.size()==0 ){
			//std::cout << "No Tris" << std::endl;
			return 0;
		}
		double minDist = DBL_MAX;
		for(size_t i=0; i<_returnFaces.size(); i++){
			//RW_ASSERT(_returnFaces.size()> i);
			//RW_ASSERT(_returnFaces.size()<vertices.size());
			//RW_ASSERT(i<_faceNormals.size());
			VectorND<3> normals;
			normals[0] = _faceNormals[i][0];
			normals[1] = _faceNormals[i][1];
			normals[2] = _faceNormals[i][2];
			double dist =  _faceOffsets[i] + dot(vertex, normals);
			// dist will be negative if point is inside, and positive if point is outside
			minDist = std::min( -dist, minDist );
		}
		return minDist;
	}

	const std::vector<rw::math::VectorND<N> >& getHullVertices(){ return _hullVertices; }

	const std::vector<int>& getFaceIndices(){ return _faceIdxs; }

	const std::vector<rw::math::VectorND<N> >& getFaceNormals(){ return _faceNormals; }

	const std::vector<double >& getFaceOffsets(){ return _faceOffsets; }

private:
	std::vector<rw::math::VectorND<N> > _hullVertices, _faceNormals;
	std::vector<rw::math::VectorND<3> > _returnFaces;
	std::vector<double> _faceOffsets;
	std::vector<int> _vertiIdxs, _faceIdxs;
	std::vector<double> _faceNormalsTmp;
};
//! @}
}
}
#endif /* GIFTWRAPHULL_HPP_ */
